#include "Util.h"
#include "Listener.h"
#include "Framework\Renderer.h"
#include "App.h"
#include "Engine.h"
#include "tokamak.h"
#include "Sprite.h"
#include "PhysicsSpriteController.h"

#define RAD_DUDE_BOX 0
#define RAD_DUDE_SPHERE 1
#define RAD_DUDE_CYLINDER 2

struct BoneData
{
	s32 geometryType;
	f32 zRotation;
	neV3 pos;
	neV3 size;
	neV3 colour;
};

char* boneMeshDefs[3][10] ={
	{
"tile_limb.torso",
"tile_limb.head",
"tile_limb.uparm",
"tile_limb.uparm",
"tile_limb.larm",
"tile_limb.larm",
"tile_limb.thigh",
"tile_limb.thigh",
"tile_limb.calf",
"tile_limb.calf"
	}
	,{
"tile_g_limb.torso",
"tile_g_limb.head",
"tile_g_limb.uparm",
"tile_g_limb.uparm.l",
"tile_g_limb.larm",
"tile_g_limb.larm.l",
"tile_g_limb.thigh",
"tile_g_limb.thigh.l",
"tile_g_limb.calf",
"tile_g_limb.calf.l"
},
{
"tile_r_limb.torso",
"tile_r_limb.head",
"tile_r_limb.uparm",
"tile_r_limb.uparm.l",
"tile_r_limb.larm",
"tile_r_limb.larm.l",
"tile_r_limb.thigh",
"tile_r_limb.thigh.l",
"tile_r_limb.calf",
"tile_r_limb.calf.l"
}
};

BoneData bones[] = 
{
	{RAD_DUDE_BOX,		0.0f, {0.0f, 0.0f, 0.0f}, {0.55f, 0.7f, 0.3f}, {0.8f, 0.2f, 0.2f}}, //body 		
	{RAD_DUDE_SPHERE,	0.0f, {0.0f, 0.55f, 0.0f}, {0.4f, 0.35f, 0.2f}, {0.8f, 0.8f, 0.2f}}, //head 

	{RAD_DUDE_CYLINDER, -NE_PI / 2.0f, {-0.45f, 0.28f, 0.0f}, {0.25f, 0.4f, 0.2f}, {0.2f, 0.2f, 0.8f}}, //right arm 
	{RAD_DUDE_CYLINDER, NE_PI / 2.0f, {0.45f, 0.28f, 0.0f}, {0.25f, 0.4f, 0.2f}, {0.2f, 0.2f, 0.8f}}, //left arm

	{RAD_DUDE_BOX, -NE_PI / 2.0f, {-0.9f, 0.28f, 0.0f}, {0.24f, 0.6f, 0.24f}, {0.2f, 0.2f, 0.8f}}, //right forearm
	{RAD_DUDE_BOX, NE_PI / 2.0f, {0.9f, 0.28f, 0.0f}, {0.24f, 0.6f, 0.24f}, {0.2f, 0.2f, 0.8f}}, //left forearm

	{RAD_DUDE_CYLINDER, 0.0f, {-0.20f, -0.6f, 0.0f}, {0.27f, 0.7f, 0.2f}, {0.3f, 0.7f, 0.2f}}, //right thigh 
	{RAD_DUDE_CYLINDER, 0.0f, {0.20f, -0.6f, 0.0f}, {0.27f, 0.7f, 0.2f}, {0.3f, 0.7f, 0.2f}}, //left thigh 

	{RAD_DUDE_BOX, 0.0f, {-0.20f, -1.3f, 0.0f}, {0.3f, 0.8f, 0.3f}, {0.3f, 0.7f, 0.2f}}, //right leg
	{RAD_DUDE_BOX, 0.0f, {0.20f, -1.3f, 0.0f}, {0.3f, 0.8f, 0.3f}, {0.3f, 0.7f, 0.2f}}, //left leg
};

enum
{
	BONE_BODY,
	BONE_HEAD,
	BONE_RIGHT_ARM,
	BONE_LEFT_ARM,
	BONE_RIGHT_FOREARM,
	BONE_LEFT_FOREARM,
	BONE_RIGHT_THIGH,
	BONE_LEFT_THIGH,
	BONE_RIGHT_LEG,
	BONE_LEFT_LEG,
};

struct JointData
{
	s32 bodyA;
	s32 bodyB;
	neV3 pos;
	s32 type; // 0 = ball joint, 1 = hinge joint
	f32 xAxisAngle;
	f32 lowerLimit;
	f32 upperLimit;
	neBool enableLimit;
	neBool enableTwistLimit;
	f32 twistLimit;
};

JointData joints[] = 
{
	{BONE_HEAD, BONE_BODY,				{0.0f, 0.35f, 0.0f}, 1, 0.0f, -NE_PI / 4.0f, NE_PI / 4.0f, true, false, 0.0f}, //head <-> body

	{BONE_RIGHT_ARM, BONE_BODY,			{-0.22f, 0.28f, 0.0f}, 0, NE_PI, 0.0f, NE_PI / 2.5f, true, true, 0.1f}, //
	{BONE_LEFT_ARM, BONE_BODY,			{0.22f, 0.28f, 0.0f}, 0, 0.0f, 0.0f, NE_PI / 2.5f, true, true, 0.1f},

	{BONE_RIGHT_FOREARM, BONE_RIGHT_ARM,{-0.65f, 0.28f, 0.0f}, 1, NE_PI, 0.0f, NE_PI / 2.0f, true, false},
	{BONE_LEFT_FOREARM, BONE_LEFT_ARM,	{0.65f, 0.28f, 0.0f}, 1, 0.0f, 0.0f, NE_PI / 2.0f, true, false},

	{BONE_RIGHT_THIGH, BONE_BODY,		{-0.20f, -0.32f, 0.0f}, 0, NE_PI * 6.0f / 8.0f, 0.0f, NE_PI / 4.0f, true, true, 0.8f},
	{BONE_LEFT_THIGH, BONE_BODY,		{0.20f, -0.32f, 0.0f}, 0, NE_PI * 2.0f / 8.0f, 0.0f, NE_PI / 4.0f, true, true, 0.8f},

	{BONE_RIGHT_LEG, BONE_RIGHT_THIGH,	{-0.20f, -0.95f, 0.0f}, 1, -NE_PI * 0.5f, -NE_PI / 2.0f, 0.0f, true, false},
	{BONE_LEFT_LEG, BONE_LEFT_THIGH,	{0.20f, -0.95f, 0.0f}, 1, -NE_PI * 0.5f, -NE_PI / 2.0f, 0.0f, true, false},
};

const s32 N_DUDE = 100;
const s32 BONES_PER_DUDE = 10;
const s32 JOINTS_PER_DUDE = 9;
const s32 N_BODY = BONES_PER_DUDE * N_DUDE;
const s32 N_STEP = 40;

neRigidBody*	rigidBodies[N_BODY];

Sprite* makeRigidBodySprite(neRigidBody*	nbody=0,char*	tilenm=null);

static unsigned int holdrand;
void msrand(unsigned int seed){
	holdrand = seed;
}

int mrand(){
	return (((holdrand = holdrand * 214013L + 2531011L) >> 16) & 0x7fff);
}

unsigned int getMSeed(){
	return holdrand;
}

Sprite* makeRadDude(neSimulator*	simulator,neV3 position, mat3 rotation,s32 & index)
{
	Sprite* rootSprite=null;
	//		const f32 groundLevel = -10.0f;
	s32 cur = 0;
	f32 scale = 1.0f;
	for (s32 i = 0; i < BONES_PER_DUDE; i++)
	{
		char ** boneMeshes=boneMeshDefs[rand()%3];
		cur = index + i;
		rigidBodies[cur] = simulator->CreateRigidBody();
		rigidBodies[cur]->CollideConnected(true);
		neV3 inertiaTensor;
		f32 mass;
		mass = 0.08f;//8.0
		if (i == 0)
			mass = 0.2f;//20.0f;
		else if (i == 8 || i ==9)
			mass = 0.08f;//8.0f;

		neGeometry * geom = rigidBodies[cur]->AddGeometry();
		Sprite* boneSprite;
		switch (bones[i].geometryType)
		{
		case RAD_DUDE_BOX:
			geom->SetBoxSize(bones[i].size[0] * scale, bones[i].size[1] * scale, bones[i].size[2] * scale);
			inertiaTensor = neBoxInertiaTensor(bones[i].size[0]* scale, 
				bones[i].size[1]* scale, 
				bones[i].size[2]* scale, mass);
			//rigidBodiesRender[cur].SetGraphicBox(bones[i].size[0] * scale, bones[i].size[1] * scale, bones[i].size[2] * scale);
			boneSprite=makeRigidBodySprite(rigidBodies[cur],boneMeshes[i]);
			break;
		case RAD_DUDE_SPHERE:
			geom->SetSphereDiameter(bones[i].size[0] * scale);
			inertiaTensor = neSphereInertiaTensor(bones[i].size[0]* scale, mass);
			//rigidBodiesRender[cur].SetGraphicSphere(bones[i].size[0] * scale * 0.5f);
			boneSprite=makeRigidBodySprite(rigidBodies[cur],boneMeshes[i]);
			break;

		case RAD_DUDE_CYLINDER:
			geom->SetCylinder(bones[i].size[0] * scale, bones[i].size[1] * scale);
			inertiaTensor = neCylinderInertiaTensor(bones[i].size[0], bones[i].size[1], mass);
			//rigidBodiesRender[cur].SetGraphicCylinder(bones[i].size[0] * scale * 0.5f, bones[i].size[1] * scale);
			boneSprite=makeRigidBodySprite(rigidBodies[cur],boneMeshes[i]);
			break;
		}
		//rigidBodiesRender[cur].SetDiffuseColor(D3DXCOLOR(bones[i].colour[0],
		//												bones[i].colour[1],
		//												bones[i].colour[2], 1));

		if(i==1)
			rootSprite=boneSprite; //Save HEAD as the root sprite
		rigidBodies[cur]->UpdateBoundingInfo();
		rigidBodies[cur]->SetInertiaTensor(inertiaTensor);
		rigidBodies[cur]->SetMass(mass);
		neV3 pos;
		pos = (bones[i].pos * scale) + position;
		rigidBodies[cur]->SetPos(pos);
		neQ quat;
		neV3 zAxis; zAxis.Set(0.0f, 0.0f, 1.0f);
		quat.Set(bones[i].zRotation, zAxis);
		rigidBodies[cur]->SetRotation(quat);
		rigidBodies[cur]->SetCollisionID(2);
		rigidBodies[cur]->SetSleepingParameter(0.5f); //make it easier to sleep
		
		rigidBodies[cur]->SetSleepingParameter(1.0f); //make it easier to sleep
		
	}

	PhysicsSpriteController* brain=(PhysicsSpriteController*)rigidBodies[index+1]->GetUserData();
	for(int t=0;t<BONES_PER_DUDE;t++){
		PhysicsSpriteController* ctl=(PhysicsSpriteController*)rigidBodies[index+t]->GetUserData();
		if(t==1){//Head
			ctl->isBrain=true;
			rigidBodies[index+t]->SetAngularDamping(0.1f);
		}else{
			ctl->parent=brain;//Set the head as the parent controller
		}
		brain->children.push_back(ctl);
	}
	neJoint * joint;

	neT3 jointFrame;

	for (s32 i = 0; i < JOINTS_PER_DUDE; i++)
	{
		joint = simulator->CreateJoint(rigidBodies[joints[i].bodyA + index], rigidBodies[joints[i].bodyB + index]);
		jointFrame.SetIdentity();
		jointFrame.pos = joints[i].pos * scale + position;

		if (joints[i].type == 0)
		{
			joint->SetType(neJoint::NE_JOINT_BALLSOCKET);
			neQ q;
			neV3 zAxis; zAxis.Set(0.0f, 0.0f, 1.0f);
			q.Set(joints[i].xAxisAngle, zAxis);
			jointFrame.rot = q.BuildMatrix3();
		}
		else
		{
			joint->SetType(neJoint::NE_JOINT_HINGE);

			if (i == 3)
			{
				jointFrame.rot[0].Set(1.0f, 0.0f, 0.0f);
				jointFrame.rot[1].Set(0.0f, 1.0f, 0.0f);
				jointFrame.rot[2].Set(0.0f, 0.0f, 1.0f);
			}
			else if (i == 4)
			{
				jointFrame.rot[0].Set(-1.0f, 0.0f, 0.0f);
				jointFrame.rot[1].Set(0.0f, -1.0f, 0.0f);
				jointFrame.rot[2].Set(0.0f, 0.0f, 1.0f);
			}
			else
			{
				jointFrame.rot[0].Set(0.0f, 0.0f, -1.0f);
				jointFrame.rot[1].Set(-1.0f, 0.0f, 0.0f);
				jointFrame.rot[2].Set(0.0f, 1.0f, 0.0f);
			}
		}

		joint->SetJointFrameWorld(jointFrame);

		if (i == 5|| i == 6) // right
		{
			neT3 body2w = rigidBodies[joints[i].bodyB+index]->GetTransform();
			neT3 w2Body = body2w.FastInverse();
			neM3 m;
			neQ q1, q2;
			neV3 zAxis; zAxis.Set(0.0f, 0.0f, 1.0f);
			q1.Set(joints[i].xAxisAngle, zAxis);
			neV3 xAxis; xAxis.Set(1.0f, 0.0f, 0.0f);
			q2.Set(-NE_PI * 0.30f, xAxis);
			neQ q; q = q2 * q1;
			m = q.BuildMatrix3();
			neT3 frame2body;
			frame2body.rot = w2Body.rot * m;
			frame2body.pos = w2Body * jointFrame.pos;
			//neT3 frame2w = body2w * frame2body;
			joint->SetJointFrameB(frame2body);
		}
		joint->SetLowerLimit(joints[i].lowerLimit);
		joint->SetUpperLimit(joints[i].upperLimit);
		if (joints[i].enableLimit)
			joint->EnableLimit(true);
		if (joints[i].enableTwistLimit)
		{
			joint->SetLowerLimit2(joints[i].twistLimit);
			joint->EnableLimit2(true);
		}
		joint->SetIteration(4);
		joint->Enable(true);
	}
	index = cur+1;
	return rootSprite;
}
